#include "pid_controllor.h"


pid_ctl::pid_ctl(const std::string& name) : name_(name) {}

void pid_ctl::set_pid(const float p, const float i, const float d, const float int_limit )
{
    kp_ = p;
    ki_ = i;
    kd_ = d;
    int_limit_ = int_limit; 
}

void pid_ctl::set_limit(const float max, const float min)
{
    out_max_ = max;
    out_min_ = min;
}

void pid_ctl::calculate(float& ouput, const float input, const float expect)
{
    float err = input - expect;
    out_.p = err * kp_;
    out_.i += err * ki_;
    if(int_limit_ > 0)
        out_.i = limit_abs(out_.i, int_limit_);
    out_.d = (err - prev_err_) / get_interval() * kd_;
    update_prev_err(err);
    ouput = out_.sum();
    last_time_ = std::chrono::system_clock::now();
}

void pid_ctl::calculate(float& ouput, const float input, const float expect, const float input_derivative)
{
    float err = input - expect;
    out_.p = err * kp_;
    out_.i += err * ki_;
    if(int_limit_ > 0)
        out_.i = limit_abs(out_.i, int_limit_);
    out_.d = input_derivative * kd_;
    update_prev_err(err);
    ouput = out_.sum();
}

float pid_ctl::limit_abs(const float input, const float max)
{
    float rst;
    if(input > fabs(max))
        rst = fabs(max);
    else if(input < -fabs(max))
        rst = -fabs(max);
    return rst;
}

float pid_ctl::limit(const float input, const float max, const float min)
{
    float rst;
    if(input > max)
        rst = max;
    else if(input < min)
        rst = min;
    return rst;
}

void pid_ctl::update_prev_err(const float err)
{
    prev_err_ = err;
}

std::string pid_ctl::param_info(void)
{
    return name_+" : [ P = " + std::to_string(kp_) + " I = "\
    + std::to_string(ki_) +  " D = " + std::to_string(kd_) + " ] ";
}


float pid_ctl::get_interval(void)
{
    unsigned long duration = std::chrono::duration_cast<std::chrono::microseconds>(\
        std::chrono::system_clock::now() - last_time_).count();
    return float(duration)/1000000.0;
}
